﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.FEZ;
using MLRobotic.IA.Drivers;
using System.Collections;
using MLRobotic.Asserv.Tools;
using GHIElectronics.NETMF.Hardware;
using MLRobotic.IA;
using Devantech.Hardware;
using MLRobotic.Asserv.Drivers;

namespace MLRobotic.Asserv
{
    public class Program
    {
        public static COM comPort;

        public static class Alert
        {
            public static bool blocageRobot = false;

        }
        private static bool obstacle = false;
        static SRF02 us1, us2;
        private static void CheckObstacle()
        {
            obstacle = (us1.Distance < 30 && us1.Distance != 0) || (us2.Distance < 30 && us2.Distance != 0);
        }

        public static void Main()
        {
            Debug.EnableGCMessages(false);



            // test des US
            us1 = new SRF02(0xF0);
            us2 = new SRF02(0xE0);
           //// us.ChangeAddress(0xF0);

           // while (true)
           // {
           //     SRF02.Process();
           //     SRF02.Process();
           //     SRF02.Process();
           //     SRF02.Process();

           //     Debug.Print("Valeur : " + us1.Distance.ToString("F0") + " - " + us2.Distance.ToString("F0"));

           //     Thread.Sleep(200);
           // }


            //OutputPort ot = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di5,true);


            //while (true) ;

            //Log.Init();
            bool fin = false;
            Queue testCibles = new Queue();

            //testCibles.Enqueue(new double[] { 400, 0 });
            //testCibles.Enqueue(new double[] { 200, 200 });
            //testCibles.Enqueue(new double[] { 400, 400 });
            //testCibles.Enqueue(new double[] { 0, 400 });
            //testCibles.Enqueue(new double[] { 0, 600 });
            //testCibles.Enqueue(new double[] { 400, 600 });
            //testCibles.Enqueue(new double[] { 0, 600 });
            //testCibles.Enqueue(new double[] { 500, 0 });
            //testCibles.Enqueue(new double[] { 500, 500 });
            //testCibles.Enqueue(new double[] { 0, 500 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 500, 0 });
            //testCibles.Enqueue(new double[] { 500, 500 });
            //testCibles.Enqueue(new double[] { 0, 500 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 500, 0 });
            //testCibles.Enqueue(new double[] { 500, 500 });
            //testCibles.Enqueue(new double[] { 0, 500 });
            //testCibles.Enqueue(new double[] { 0, 0 });


            Ecran.Init();
            //Ecran.Clear();
            //Ecran.Print("X:" + Deplace.Xpos.ToString("N0"));
            //Ecran.SetCursor(0, 8);
            //Ecran.Print("Y:" + Deplace.Xpos.ToString("N0"));
            //Ecran.SetCursor(1, 0);
            //Ecran.Print("Teta:" + Deplace.TetaPos.ToString("N4"));

            Deplace.Init();


            Deplace.TestMoteurs();
            while (true)
            {


                Deplace.CalculPosition();
            }

            comPort = new COM("COM3");


            while (true)
            {
                // Boucle principale
                Deplace.CalculPosition();
                Deplace.DeplaceBouge();

                comPort.Process();

                SRF02.Process();
                CheckObstacle();

                //Deplace.CalculPID();
                //Ecran.Process();


                // pour test :
                if (Deplace.ArriveACible && testCibles.Count > 0)
                {
                    double[] cible = (double[])testCibles.Dequeue();
                    // pour test : next cible
                    if (cible.Length == 2)
                    {
                        //sdcard.write("goto : " + cible[0].tostring() + ", " + cible[1].tostring());
                        Deplace.GotoXY(cible[0], cible[1], true);
                    }
                    else
                    {
                        //sdcard.write("goto : " + cible[0].tostring("n3"));
                        Deplace.GotoTeta(cible[0]);
                    }

                    //ecran.clear();
                    //ecran.print("x:" + deplace.xpos.tostring("n0"));
                    //ecran.setcursor(0, 8);
                    //ecran.print("y:" + deplace.xpos.tostring("n0"));
                    //ecran.setcursor(1, 0);
                    //ecran.print("teta:" + deplace.tetapos.tostring("n4"));
                }
                else if (Deplace.ArriveACible && !fin)
                {
                    fin = true;
                    Deplace.CalculPosition();
                    //sdcard.write("posfin : " + deplace.xpos.tostring("n0") + ";" + deplace.ypos.tostring("n0") + ";" + deplace.tetapos.tostring("f4"));
                    //ecran.clear();
                    //ecran.print("x:" + deplace.xpos.tostring("n0"));
                    //ecran.setcursor(0, 8);
                    //ecran.print("y:" + deplace.xpos.tostring("n0"));
                    //ecran.setcursor(1, 0);
                    //ecran.print("teta:" + deplace.tetapos.tostring("n4"));
                }

                if (Deplace.alertDerapage)
                {
                    Debug.Print("Clear Derrapage");
                    testCibles.Clear();
                }

                if (obstacle)
                {
                    Debug.Print("OBSTACLE !!!!!!");
                    Deplace.Stop();
                    testCibles.Clear();
                }
            }
        }


        //#region Gestion des codeurs
        //static System.Threading.Timer tiCod;
        //private static void InitCodeurs()
        //{
        //    tiCod = new System.Threading.Timer(new System.Threading.TimerCallback(UpdateCodeurs), null, tempo, tempo);
        //}
        //static int tempo = 100;

        //public static double coefCodD = 1;
        //public static double coefCodG = 1;


        //private static void UpdateCodeurs(object sender)
        //{
        //    Deplace.codeurDroit += Convert.ToInt32((tempo * Deplace.curvalDroit / 1000 * coefCodD).ToString());
        //    Deplace.codeurGauche += Convert.ToInt32((tempo * Deplace.curvalGauche / 1000 * coefCodG).ToString());

        //}
        //#endregion


    }
}
